#include "vehicle_lateral_speed3dis.h"

namespace auto_ros
{
namespace control
{
VehicleLateralSpeed3dis::VehicleLateralSpeed3dis(std::string dyn_yaml_path_name)
	: VehicleDynBase(dyn_yaml_path_name, 4, 1, 3, 1)
{
	mass_ = dyn_yaml_["mass"].as<double>();
	cf_ = dyn_yaml_["cf"].as<double>();
	cr_ = dyn_yaml_["cr"].as<double>();
	lf_ = dyn_yaml_["lf"].as<double>();
	lr_ = dyn_yaml_["lr"].as<double>();
	iz_ = dyn_yaml_["iz"].as<double>();
	max_lateral_acc_ = dyn_yaml_["max_lateral_acc"].as<double>();
	max_tire_f_angle_ = dyn_yaml_["max_tire_f_angle"].as<double>();
	speed_threshold_ = dyn_yaml_["speed_threshold"].as<double>();

	c_yaw_rate_ = -lf_ / 1.0 * cf_ + lr_ / 1.0 * cr_;
	c_beta_ = -(cf_ + cr_);
	c_delta_ = cf_;

	d_yaw_rate_ = -lf_ * lf_ / 1.0 * cf_ - lr_ * lr_ / 1.0 * cr_;
	d_delta_ = lf_ * cf_;
	d_beta_ = -lf_ * cf_ + lr_ * cr_;
	speed_threshold_ = 1.0;
	//init speed is 1.0m/s
	state_matrix_ << c_beta_ / mass_ / 1.0, c_yaw_rate_ / mass_ - 1.0, 0, 0,
		d_beta_ / iz_ / 1.0, d_yaw_rate_ / iz_, 0, 0,
		0, 0, 0, 1,
		c_beta_ / mass_ / 1.0, c_yaw_rate_ / mass_, 0, 0;
	control_matrix_ << c_delta_ / mass_,
		d_delta_ / iz_,
		0,
		c_delta_ / mass_;
	distur_matrix_ << 0, 1, 0,
		0, 0, 1,
		0, 0, 0,
		-1.0 * 1.0, 1, 0;
	out_state_matrix_ << 0, 0, 1, 0;
	out_distur_matrix_ << 0, 0, 0;
}
void VehicleLateralSpeed3dis::speed_update_matrix(double speed)
{
	if (std::abs(speed) < speed_threshold_)
	{
		if (speed >= 0)
			speed = speed_threshold_;
		else
			speed = -speed_threshold_;
	}
	speed_ = speed;
	c_yaw_rate_ = -lf_ / speed * cf_ + lr_ / speed * cr_;
	d_yaw_rate_ = -lf_ * lf_ / speed * cf_ - lr_ * lr_ / speed * cr_;
	state_matrix_ << c_beta_ / mass_ / speed, c_yaw_rate_ / mass_ - speed, 0, 0,
		d_beta_ / iz_ / speed, d_yaw_rate_ / iz_, 0, 0,
		0, 0, 0, 1,
		c_beta_ / mass_ / speed, c_yaw_rate_ / mass_, 0, 0;
	control_matrix_ << c_delta_ / mass_,
		d_delta_ / iz_,
		0,
		c_delta_ / mass_;
	distur_matrix_(3, 0) = -speed * speed;
}

double VehicleLateralSpeed3dis::mass()
{
	return mass_;
}
double VehicleLateralSpeed3dis::lf()
{
	return lf_;
}
double VehicleLateralSpeed3dis::lr()
{
	return lr_;
}
double VehicleLateralSpeed3dis::cf()
{
	return cf_;
}
double VehicleLateralSpeed3dis::cr()
{
	return cr_;
}
double VehicleLateralSpeed3dis::iz()
{
	return iz_;
}
double VehicleLateralSpeed3dis::max_tire_f_angle()
{
	return max_tire_f_angle_;
}
double VehicleLateralSpeed3dis::speed_threshold()
{
	return speed_threshold_;
}

} // namespace control
} // namespace auto_ros